/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/

#define MAP_FILE_NAME "../../res/autmap-small.txt"

#define PROCESS_MCL

#include<stdio.h>
#include<stdlib.h>
#include<math.h>

#include "TOccupancyGrid.h" ///< Occupancy grid class
#include "CParticleFilter.h"
#include "glutImageGUI.h"   ///< For visualisation
#include "JPEGEncoder.hpp"  ///< For saving JPEG

TOccupancyGrid *O;
		
float topi(float da){
	if(da>M_PI)while(da>M_PI) da-=2.0*M_PI;
	else if(da<-M_PI)while(da<-M_PI) da+=2.0*M_PI;
	return da;
}
//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
///DRAWING STUFF
//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
void setRGBpix(unsigned char *rgb,int i,int j, int Nx, unsigned char r,unsigned char g, unsigned char b ){
		rgb[3*Nx*j+3*i]=r;
		rgb[3*Nx*j+3*i+1]=g;
		rgb[3*Nx*j+3*i+2]=b;
}

void setRGBmap(unsigned char *rgb,TOccupancyGrid *gr, unsigned char r,unsigned char g, unsigned char b){
		float *data = gr->getDataPtr();
		int Nx,Ny;
		gr->getDim(Nx,Ny);
		float v;
	
		for(int j=0;j<Ny;j++){
				for(int i=0;i<Nx;i++){
						v = data[Nx*j+i];
						if(v>0.6) v = 0; else v = 0.99; 
				
						setRGBpix(rgb,i,j, Nx, r*v,g*v,b*v);
				}
		}	
}

void setRGBpixBold(unsigned char *rgb,int i,int j, int Nx, unsigned char r,unsigned char g, unsigned char b){
		for(int ii=i-1;ii<i+1;ii++){
				for(int jj=j-1;jj<j+1;jj++){
						setRGBpix(rgb,ii,jj,Nx,r,g,b);
				}
		}

}

void drawLine(unsigned char *RGB, int Nx,int Ny,float resolution,   ///Grid parameters
							uint pix, uint piy,float angle, float length, ///< Start pixel, angle to draw and linelength
							unsigned char r,unsigned char g,unsigned char b){
									float dy = sin(angle);
									float dx = cos(angle);
									float rr=0;
									int i=0;
									uint x,y;
									while(rr<length){
											x = pix + i*dx;
											y = piy - i*dy;
											setRGBpixBold(RGB,x,y, Nx, r,g,b);
											i++;
											rr+=resolution;
									}
}

void drawLineThin(unsigned char *RGB, int Nx,int Ny,float resolution,   ///Grid parameters
							uint pix, uint piy,float angle, float length, ///< Start pixel, angle to draw and linelength
							unsigned char r,unsigned char g,unsigned char b){
									float dy = sin(angle);
									float dx = cos(angle);
									float rr=0;
									int i=0;
									uint x,y;
									while(rr<length){
											x = pix + i*dx;
											y = piy - i*dy;
											setRGBpix(RGB,x,y, Nx, r,g,b);
											i++;
											rr+=resolution;
									}
							}

void drawCross(unsigned char *RGB, int Nx,int Ny,float resolution,   ///Grid parameters
							uint pix, uint piy, float length, ///< Start pixel, angle to draw and linelength
							unsigned char r,unsigned char g,unsigned char b){
		float angle = M_PI/4;
		for(int i=0;i<3;i++){
				drawLineThin(RGB, Nx,Ny,resolution, pix, piy,angle, length, r,g,b);
				angle+=M_PI/2;
		}
}


void saveJPEG(const char *fname, unsigned char *rgb,int Nx, int Ny){
		CJPEGEncoder encoder;
		encoder.SetRGBDataPtr((const char *)rgb,Nx,Ny);
		FILE *f = fopen(fname,"wb");
		fwrite(encoder.GetJPEGDataPtr(), 
					 encoder.GetJPEGDataSize(), 
					 1, f);
		fclose(f);
		
}

float randomi(){
	return (2.0 * (float)(rand()%10000) - 1.0)/10000.0;
}

//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////

void saveScan(const char *fname, Grid::TScan &s){
		FILE *f = fopen(fname,"wt");
		if(f==NULL){
				fprintf(stderr,"Not good - unable to save!\n");
				exit(1);
		}

		for(int i=0;i<s.N;i++){
				fprintf(f,"%f %f\n",s.r[i],s.a[i]);
		}
		fclose(f);
}

void drawRobot(unsigned char *rgb, TOccupancyGrid *O, Grid::pose pos, unsigned char r,unsigned char g, unsigned char b ){
	Grid::TScan s;
	s.alloc(181);
	Grid::TCoord2D ocoord;
	Grid::TPixel pix;
	float range;
	
	
	int Nx,Ny;
	O->getDim(Nx,Ny);
	
	O->getScan(pos.x,pos.y, pos.a-M_PI/2.0, 181, M_PI/180, 0.9, s);
	for(int i=0;i<181;i++){
				if(s.r[i] >0){
					range = s.r[i] + 0.15*randomi();
					ocoord.x = pos.x +  range * cos(s.a[i]+pos.a-M_PI/2.0) ;ocoord.y = pos.y+range * sin(s.a[i]+pos.a-M_PI/2.0);
					pix = O->toPixel(ocoord);
					drawCross(rgb, Nx,Ny,O->getResolution(),pix.x,pix.y, 0.2, r,g,b);
				}
	}
	
	ocoord.x = pos.x ;ocoord.y = pos.y;
	pix = O->toPixel(ocoord);
	drawLine(rgb, Nx,Ny, O->getResolution(),pix.x,pix.y,pos.a, 1.0,r,g,b);
	drawLine(rgb, Nx,Ny, O->getResolution(),pix.x,pix.y,pos.a+M_PI/2, 0.5,r,g,b);
	drawLine(rgb, Nx,Ny, O->getResolution(),pix.x,pix.y,pos.a-M_PI/2, 0.5,r,g,b);
			
	
}


int main(int argc, char *argv[]){
		Grid::TCoord2D ocoord;
		Grid::TPixel pix;
		Grid::TScan s,m;
		int Nx,Ny;
		
		s.alloc(181);
		m.alloc(181);

		O = new TOccupancyGrid(1,1,0.04,Grid::TCoord2D(0,0));
		if(!O->loadLineMap(MAP_FILE_NAME, 1.0,1.0)){
				fprintf(stderr,"**** Unable to load the map\n");
				exit(1);
		}
				
		
		O->getDim(Nx,Ny);
		unsigned char *rgb = (unsigned char *) malloc(3*Nx*Ny);
		addImageWindow(Nx, Ny);
		glutImageGUI_Start();
		
		setRGBmap(rgb,O, 220,220, 220);
		
		displayImage(0, rgb,Ny,Nx);
		
	
		float x_ref = 0, y_ref=-1.0, a_ref=45.0*M_PI/180.0;
		float dx = -4.; 
		float dy = 3.2;
		float da = 60.0*M_PI/180;
		
		int cnt=0;
		float r;
		while(1){
			setRGBmap(rgb,O, 240,240, 255);
			drawRobot(rgb, O, Grid::pose(0,0,M_PI/5), 255,0, 0 );
			drawRobot(rgb, O, Grid::pose(dx,dy,da), 0,255, 0 );
			drawRobot(rgb, O, Grid::pose(20,5,4.5), 0,0, 255 );
			
			displayImage(0, rgb,Ny,Nx);
			if(cnt==0) {
				saveJPEG("scans.jpg",rgb,Nx,Ny);
				mcl::pose diff;
				diff.setToDifferentialPose(mcl::pose(x_ref+dx,y_ref+dy,a_ref+da),mcl::pose(x_ref,y_ref,a_ref));
				fprintf(stderr,"DIFF (%.3f, %.3f, %.3f)\n",diff.x,diff.y,diff.a);	
				
				//diff.setToDifferentialPose(mcl::pose(x_ref,y_ref,a_ref),mcl::pose(x_ref+dx,y_ref+dy,a_ref+da));
				//fprintf(stderr,"DIFF2 (%.3f, %.3f, %.3f)\n",diff.x,diff.y,diff.a);	
				
				mcl::pose relative;
				relative.set(0,0,0);
				diff = relative.integrateDifferential(diff);
				fprintf(stderr,"RELA (%.3f, %.3f, %.3f)\n",diff.x,diff.y,diff.a);
				
				saveScan("refScan.txt",s);
				saveScan("actScan.txt",m);
			}
			cnt++;
			usleep(100*1000);
		}
		
		//saveJPEG("odo_result.jpg",rgb,Nx,Ny);		
		return 0;
}		